//
// File: demo01.cpp
//
// Code generated for Simulink model 'demo01'.
//
// Model version                  : 1.6
// Simulink Coder version         : 9.7 (R2022a) 13-Nov-2021
// C/C++ source code generated on : Tue May  7 21:14:21 2024
//
// Target selection: ert.tlc
// Embedded hardware selection: Generic->Unspecified (assume 32-bit Generic)
// Code generation objectives: Unspecified
// Validation result: Not run
//
#include "demo01.h"
#include "rtwtypes.h"

// Block signals (default storage)
B_demo01_T demo01_B;

// Block states (default storage)
DW_demo01_T demo01_DW;

// Real-time model
RT_MODEL_demo01_T demo01_M_ = RT_MODEL_demo01_T();
RT_MODEL_demo01_T *const demo01_M = &demo01_M_;

// Model step function
void demo01_step(void)
{
  real_T rtb_Filter;
  real_T rtb_Filter_idx_0;
  real_T rtb_Sum_idx_0;
  real_T rtb_Sum_l_idx_0;
  real_T rtb_Sum_n;
  boolean_T b_varargout_1;

  // Outputs for Atomic SubSystem: '<Root>/Subscribe'
  // MATLABSystem: '<S6>/SourceBlock'
  b_varargout_1 = Sub_demo01_8.getLatestMessage(&demo01_B.b_varargout_2);

  // Outputs for Enabled SubSystem: '<S6>/Enabled Subsystem' incorporates:
  //   EnablePort: '<S56>/Enable'

  if (b_varargout_1) {
    // SignalConversion generated from: '<S56>/In1'
    demo01_B.In1_k = demo01_B.b_varargout_2;
  }

  // End of MATLABSystem: '<S6>/SourceBlock'
  // End of Outputs for SubSystem: '<S6>/Enabled Subsystem'
  // End of Outputs for SubSystem: '<Root>/Subscribe'

  // Outputs for Atomic SubSystem: '<Root>/Subscribe1'
  // MATLABSystem: '<S7>/SourceBlock'
  b_varargout_1 = Sub_demo01_10.getLatestMessage(&demo01_B.b_varargout_2);

  // Outputs for Enabled SubSystem: '<S7>/Enabled Subsystem' incorporates:
  //   EnablePort: '<S57>/Enable'

  if (b_varargout_1) {
    // SignalConversion generated from: '<S57>/In1'
    demo01_B.In1 = demo01_B.b_varargout_2;
  }

  // End of MATLABSystem: '<S7>/SourceBlock'
  // End of Outputs for SubSystem: '<S7>/Enabled Subsystem'
  // End of Outputs for SubSystem: '<Root>/Subscribe1'

  // Sum: '<Root>/Sum' incorporates:
  //   SignalConversion generated from: '<Root>/Vector Concatenate1'
  //   SignalConversion generated from: '<Root>/Vector Concatenate'

  rtb_Sum_n = demo01_B.In1_k.Pose.Position.X - demo01_B.In1.Pose.Position.X;

  // Gain: '<S42>/Filter Coefficient' incorporates:
  //   DiscreteIntegrator: '<S34>/Filter'
  //   Gain: '<S33>/Derivative Gain'
  //   Sum: '<S34>/SumD'

  rtb_Filter = (demo01_P.DiscretePIDController_D * rtb_Sum_n -
                demo01_DW.Filter_DSTATE[0]) * demo01_P.DiscretePIDController_N;

  // Sum: '<S48>/Sum' incorporates:
  //   DiscreteIntegrator: '<S39>/Integrator'
  //   Gain: '<S44>/Proportional Gain'

  rtb_Sum_idx_0 = (demo01_P.DiscretePIDController_P * rtb_Sum_n +
                   demo01_DW.Integrator_DSTATE[0]) + rtb_Filter;

  // Sum: '<Root>/Sum'
  rtb_Sum_l_idx_0 = rtb_Sum_n;

  // Gain: '<S42>/Filter Coefficient'
  rtb_Filter_idx_0 = rtb_Filter;

  // Sum: '<Root>/Sum' incorporates:
  //   SignalConversion generated from: '<Root>/Vector Concatenate1'
  //   SignalConversion generated from: '<Root>/Vector Concatenate'

  rtb_Sum_n = demo01_B.In1_k.Pose.Position.Y - demo01_B.In1.Pose.Position.Y;

  // Gain: '<S42>/Filter Coefficient' incorporates:
  //   DiscreteIntegrator: '<S34>/Filter'
  //   Gain: '<S33>/Derivative Gain'
  //   Sum: '<S34>/SumD'

  rtb_Filter = (demo01_P.DiscretePIDController_D * rtb_Sum_n -
                demo01_DW.Filter_DSTATE[1]) * demo01_P.DiscretePIDController_N;

  // BusAssignment: '<Root>/Bus Assignment' incorporates:
  //   Constant: '<S1>/Constant'
  //   DiscreteIntegrator: '<S39>/Integrator'
  //   Gain: '<S44>/Proportional Gain'
  //   Sum: '<S48>/Sum'

  demo01_B.BusAssignment = demo01_P.Constant_Value_d;
  demo01_B.BusAssignment.Linear.X = rtb_Sum_idx_0;
  demo01_B.BusAssignment.Linear.Y = (demo01_P.DiscretePIDController_P *
    rtb_Sum_n + demo01_DW.Integrator_DSTATE[1]) + rtb_Filter;

  // Outputs for Atomic SubSystem: '<Root>/Publish'
  // MATLABSystem: '<S4>/SinkBlock'
  Pub_demo01_1.publish(&demo01_B.BusAssignment);

  // End of Outputs for SubSystem: '<Root>/Publish'

  // BusAssignment: '<Root>/Bus Assignment1' incorporates:
  //   Constant: '<S2>/Constant'

  demo01_B.BusAssignment1 = demo01_P.Constant_Value_h;
  demo01_B.BusAssignment1.Position.X = rtb_Sum_l_idx_0;
  demo01_B.BusAssignment1.Position.Y = rtb_Sum_n;

  // Outputs for Atomic SubSystem: '<Root>/Publish1'
  // MATLABSystem: '<S5>/SinkBlock'
  Pub_demo01_18.publish(&demo01_B.BusAssignment1);

  // End of Outputs for SubSystem: '<Root>/Publish1'

  // Update for DiscreteIntegrator: '<S39>/Integrator' incorporates:
  //   Gain: '<S36>/Integral Gain'

  demo01_DW.Integrator_DSTATE[0] += demo01_P.DiscretePIDController_I *
    rtb_Sum_l_idx_0 * demo01_P.Integrator_gainval;

  // Update for DiscreteIntegrator: '<S34>/Filter'
  demo01_DW.Filter_DSTATE[0] += demo01_P.Filter_gainval * rtb_Filter_idx_0;

  // Update for DiscreteIntegrator: '<S39>/Integrator' incorporates:
  //   Gain: '<S36>/Integral Gain'

  demo01_DW.Integrator_DSTATE[1] += demo01_P.DiscretePIDController_I * rtb_Sum_n
    * demo01_P.Integrator_gainval;

  // Update for DiscreteIntegrator: '<S34>/Filter'
  demo01_DW.Filter_DSTATE[1] += demo01_P.Filter_gainval * rtb_Filter;
}

// Model initialize function
void demo01_initialize(void)
{
  {
    char_T b_zeroDelimTopic_1[16];
    char_T b_zeroDelimTopic_0[13];
    char_T b_zeroDelimTopic[11];
    char_T b_zeroDelimTopic_2[4];
    static const char_T tmp[10] = { '/', 'u', 'g', 'v', '1', '/', 'p', 'o', 's',
      'e' };

    static const char_T tmp_0[12] = { '/', 't', 'e', 'l', 'l', 'o', '1', '/',
      'p', 'o', 's', 'e' };

    static const char_T tmp_1[15] = { '/', 't', 'e', 'l', 'l', 'o', '1', '/',
      'c', 'm', 'd', '_', 'v', 'e', 'l' };

    // InitializeConditions for DiscreteIntegrator: '<S39>/Integrator'
    demo01_DW.Integrator_DSTATE[0] = demo01_P.DiscretePIDController_Initial_d;

    // InitializeConditions for DiscreteIntegrator: '<S34>/Filter'
    demo01_DW.Filter_DSTATE[0] = demo01_P.DiscretePIDController_InitialCo;

    // InitializeConditions for DiscreteIntegrator: '<S39>/Integrator'
    demo01_DW.Integrator_DSTATE[1] = demo01_P.DiscretePIDController_Initial_d;

    // InitializeConditions for DiscreteIntegrator: '<S34>/Filter'
    demo01_DW.Filter_DSTATE[1] = demo01_P.DiscretePIDController_InitialCo;

    // SystemInitialize for Atomic SubSystem: '<Root>/Subscribe'
    // SystemInitialize for Enabled SubSystem: '<S6>/Enabled Subsystem'
    // SystemInitialize for SignalConversion generated from: '<S56>/In1' incorporates:
    //   Outport: '<S56>/Out1'

    demo01_B.In1_k = demo01_P.Out1_Y0;

    // End of SystemInitialize for SubSystem: '<S6>/Enabled Subsystem'

    // Start for MATLABSystem: '<S6>/SourceBlock'
    demo01_DW.obj_e3.matlabCodegenIsDeleted = false;
    demo01_DW.obj_e3.isInitialized = 1;
    for (int32_T i = 0; i < 10; i++) {
      b_zeroDelimTopic[i] = tmp[i];
    }

    b_zeroDelimTopic[10] = '\x00';
    Sub_demo01_8.createSubscriber(&b_zeroDelimTopic[0], 1);
    demo01_DW.obj_e3.isSetupComplete = true;

    // End of Start for MATLABSystem: '<S6>/SourceBlock'
    // End of SystemInitialize for SubSystem: '<Root>/Subscribe'

    // SystemInitialize for Atomic SubSystem: '<Root>/Subscribe1'
    // SystemInitialize for Enabled SubSystem: '<S7>/Enabled Subsystem'
    // SystemInitialize for SignalConversion generated from: '<S57>/In1' incorporates:
    //   Outport: '<S57>/Out1'

    demo01_B.In1 = demo01_P.Out1_Y0_p;

    // End of SystemInitialize for SubSystem: '<S7>/Enabled Subsystem'

    // Start for MATLABSystem: '<S7>/SourceBlock'
    demo01_DW.obj_n.matlabCodegenIsDeleted = false;
    demo01_DW.obj_n.isInitialized = 1;
    for (int32_T i = 0; i < 12; i++) {
      b_zeroDelimTopic_0[i] = tmp_0[i];
    }

    b_zeroDelimTopic_0[12] = '\x00';
    Sub_demo01_10.createSubscriber(&b_zeroDelimTopic_0[0], 1);
    demo01_DW.obj_n.isSetupComplete = true;

    // End of Start for MATLABSystem: '<S7>/SourceBlock'
    // End of SystemInitialize for SubSystem: '<Root>/Subscribe1'

    // SystemInitialize for Atomic SubSystem: '<Root>/Publish'
    // Start for MATLABSystem: '<S4>/SinkBlock'
    demo01_DW.obj_e.matlabCodegenIsDeleted = false;
    demo01_DW.obj_e.isInitialized = 1;
    for (int32_T i = 0; i < 15; i++) {
      b_zeroDelimTopic_1[i] = tmp_1[i];
    }

    b_zeroDelimTopic_1[15] = '\x00';
    Pub_demo01_1.createPublisher(&b_zeroDelimTopic_1[0], 1);
    demo01_DW.obj_e.isSetupComplete = true;

    // End of Start for MATLABSystem: '<S4>/SinkBlock'
    // End of SystemInitialize for SubSystem: '<Root>/Publish'

    // SystemInitialize for Atomic SubSystem: '<Root>/Publish1'
    // Start for MATLABSystem: '<S5>/SinkBlock'
    demo01_DW.obj.matlabCodegenIsDeleted = false;
    demo01_DW.obj.isInitialized = 1;
    b_zeroDelimTopic_2[0] = '/';
    b_zeroDelimTopic_2[1] = 'e';
    b_zeroDelimTopic_2[2] = '1';
    b_zeroDelimTopic_2[3] = '\x00';
    Pub_demo01_18.createPublisher(&b_zeroDelimTopic_2[0], 1);
    demo01_DW.obj.isSetupComplete = true;

    // End of SystemInitialize for SubSystem: '<Root>/Publish1'
  }
}

// Model terminate function
void demo01_terminate(void)
{
  // Terminate for Atomic SubSystem: '<Root>/Subscribe'
  // Terminate for MATLABSystem: '<S6>/SourceBlock'
  if (!demo01_DW.obj_e3.matlabCodegenIsDeleted) {
    demo01_DW.obj_e3.matlabCodegenIsDeleted = true;
  }

  // End of Terminate for MATLABSystem: '<S6>/SourceBlock'
  // End of Terminate for SubSystem: '<Root>/Subscribe'

  // Terminate for Atomic SubSystem: '<Root>/Subscribe1'
  // Terminate for MATLABSystem: '<S7>/SourceBlock'
  if (!demo01_DW.obj_n.matlabCodegenIsDeleted) {
    demo01_DW.obj_n.matlabCodegenIsDeleted = true;
  }

  // End of Terminate for MATLABSystem: '<S7>/SourceBlock'
  // End of Terminate for SubSystem: '<Root>/Subscribe1'

  // Terminate for Atomic SubSystem: '<Root>/Publish'
  // Terminate for MATLABSystem: '<S4>/SinkBlock'
  if (!demo01_DW.obj_e.matlabCodegenIsDeleted) {
    demo01_DW.obj_e.matlabCodegenIsDeleted = true;
  }

  // End of Terminate for MATLABSystem: '<S4>/SinkBlock'
  // End of Terminate for SubSystem: '<Root>/Publish'

  // Terminate for Atomic SubSystem: '<Root>/Publish1'
  // Terminate for MATLABSystem: '<S5>/SinkBlock'
  if (!demo01_DW.obj.matlabCodegenIsDeleted) {
    demo01_DW.obj.matlabCodegenIsDeleted = true;
  }

  // End of Terminate for MATLABSystem: '<S5>/SinkBlock'
  // End of Terminate for SubSystem: '<Root>/Publish1'
}

//
// File trailer for generated code.
//
// [EOF]
//
